import numpy as np
import cv2
import BinocularMeas as bl
import glob

# 读取相机内外参的txt文件
file = open('Parameters.txt', 'r')
P = (np.array([x.strip() for x in file.readlines()])).astype(np.float64)
# 图像的分辨率
imageSize = (1280, 1024)


# # 导轨实1
imgL = cv2.imread('L1.bmp', 0)
imgR = cv2.imread('R1.bmp', 0)

imgL2 = cv2.imread('L2.bmp', 0)
imgR2 = cv2.imread('R2.bmp', 0)
# 光斑的三维坐标
w = np.array(bl.calCoordinateFromSpotcentroid(P, imgL, imgR))
w2 = np.array(bl.calCoordinateFromSpotcentroid(P, imgL2, imgR2))

distance = np.sqrt((w[0] - w2[0]) ** 2 + (w[1] - w2[1]) ** 2 + (w[2] - w2[2]) ** 2)

print(w)
print(w2)
print(distance)
